Fair Cake Cutter

import cv2
import numpy as np
import time
import subprocess
import os
import sys, pygame
from picamera import PiCamera

start = time.time()
tlim = 20
#to pitft
os.putenv('SDL_VIDEODRIVER', 'fbcon')
os.putenv('SDL_FBDEV', '/dev/fb0')
os.putenv('SDL_MOUSEDRV', 'TSLIB')
os.putenv('SDL_MOUSEDEV', '/dev/input/touchscreen')
#pygame setup
pygame.init()

size = width,height = 320,240
screen = pygame.display.set_mode(size)
white = 255, 255, 255
black = 0, 0, 0
screen.fill(black)
pygame.mouse.set_visible(False)
#big font
big_font=pygame.font.Font(None,30)
buttons = {"center the cake better on the plate": (160, 20),
	"the program will proceed when well-centered": (160,40)} #OR SAY "press button 17 when well centered" for GPIO
#GPIO setup - may not need
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP)
def GPIO17_callback(channel):
        global waiting
	waiting = False
GPIO.add_event_detect(17,GPIO.FALLING,callback=GPIO17_callback)

#finds circles on an image
def circles(img):
	resized_img = cv2.resize(img,(320,240))
	# convert to grayscale
	gray = cv2.cvtColor(resized_img,cv2.COLOR_BGR2GRAY)
	#img_rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
	# blur using 3x3 kernel
	gray_blurred = cv2.blur(gray,(3,3))
	#apply hough transform on the blurred image
	detected_circles = cv2.HoughCircles(gray_blurred, cv2.HOUGH_GRADIENT, 1, 200, param1 = 50,
	               param2 = 30, minRadius = 200, maxRadius = 1000)
	x_arr = []
	y_arr = []
	r_arr = []
	# draw circles that are detected
	if detected_circles is not None:
	        # convert the circle parameters a, b, and r to integers
	        detected_circles = np.uint16(np.around(detected_circles))
		i = 0
	        for pt in detected_circles[0,:]:
			x_arr[i] = pt[0]
			y_arr[i] = pt[1]
			r_arr[i] = pt[2]
			i = i+1
	                cv2.circle(img,(pt[0],pt[1]),1,(0,0,255),3)
	                cv2.circle(img,(pt[0],pt[1]),pt[2],(0,255,0),4)
	                cv2.imshow("Detected Circle",img)
	                cv2.waitKey(0)
	return x_arr,y_arr,r_arr
j = 0
#cap = cv2.VideoCapture(0)
centered = False
waiting = True
print(j)
camera = PiCamera()
#camera.start
while ( not centered) and ( time.time() < start+tlim):
	print(j)
	j = j+1
	waiting = True
        time.sleep(0.01)
        screen.fill(black)
#	ret, pic = cap.read()
	#take picture
#	camera = PiCamera()
	camera.start_preview(alpha=255)
	time.sleep (1)
	print('take now')
	camera.capture('/home/pi/imagecake.jpg')
	camera.stop_preview()
	pic = cv2.imread('/home/pi/imagecake.jpg')
#	cv2.imshow('window', pic)
#	cv2.waitKey(0)
        #### process picture ###
	result = circles(pic)
	x_arr = result[0]
	y_arr = result[1]
	r_arr = result[2]
	#find base and cake parameters - assumes cake is second largest circle
	if len(r_arr) > 0:
		base_r = max(r_arr)
		base_ind = r_arr.index(base_r)
		base_x = x_arr.pop(base_ind)
		base_y = y_arr.pop(base_ind)
		r_arr.pop(base_ind)
		cake_r = max(r_arr)
		cake_ind = r_arr.index(cake_r)
        	cake_x = x_arr[cake_ind]
        	cake_y = y_arr[cake_ind]
		cake_error = np.sqrt((cake_x-base_x)**2 + (cake_y-base_y)**2)
		#if cake center off by >8% of plate radius (6.8 in), ask to recenter
		if cake_error > 0.08*base_r:
			for text, position in buttons.items():
			        display_text = big_font.render(text, True, white)
        			rect = display_text.get_rect(center=position)
        			screen.blit(display_text, rect)
			#uncomment to wait for user input
		#	while waiting:
		#		time.sleep(0.05)
		else:
			centered = True
			break
	else:
		display_text = big_font.render("No circles found, try again!", True, white)
		rect = display_text.get_rect(center=(160,120))
		screen.blit(display_text,rect)
	cv2.destroyAllWindows()
	pygame.display.flip()
print("Done!")
GPIO.cleanup()
#cap.release()
cv2.destroyAllWindows()

# Ching Pang (cp546) Michelle DeSouza (md722)
import RPi.GPIO as GPIO
import time
import os
import sys, pygame
import cv2
import numpy as np
import subprocess

# showing on piTFT
os.putenv('SDL_VIDEORIVER','fbcon')
os.putenv('SDL_FBDEV','/dev/fb0')
os.putenv('SDL_MOUSEDRV','TSLIB')
os.putenv('SDL_MOUSEDEV','/dev/input/touchscreen')

GPIO.setmode(GPIO.BCM)
# bail out button
GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP)
# correspond to on screen button for level 2
#GPIO.setup(27, GPIO.IN, pull_up_down=GPIO.PUD_UP)
#GPIO.setup(23, GPIO.IN, pull_up_down=GPIO.PUD_UP)
#GPIO.setup(22, GPIO.IN, pull_up_down=GPIO.PUD_UP)
def GPIO17_callback(channel):
	global running
	print("quit with bail out button")
	running = 0
	GPIO.cleanup()
# on screen buttons for level 2 detection
#GPIO.add_event_detect(27,GPIO.FALLING, callback=GPIO27_callback)
#GPIO.add_event_detect(23,GPIO.FALLING, callback=GPIO23_callback)
#GPIO.add_event_detect(22,GPIO.FALLING, callback=GPIO22_callback)
# bail out button detection
GPIO.add_event_detect(17,GPIO.FALLING,callback=GPIO17_callback,bouncetime=400)

# pygame setup choosing number
pygame.init()

size = width,height = 320,240
screen = pygame.display.set_mode(size)
small_font = pygame.font.Font(None, 25)
big_font = pygame.font.Font(None, 35)
white = 255,255,255
black = 0,0,0
red = 255,0,0
green = 0, 255, 0
pygame.mouse.set_visible(False)
global running
running = 1
num = "01"
#global num
#global numhist
numhist = ["01"]
# buttons and positions on screen
buttons = {"1": (25,160),"2": (75,160),"3": (125,160),"4": (175,160),"5": (225,160),
	"6":(25,200),"7":(75,200),"8":(125,200),"9":(175,200),"0":(225,200),
	"confirm":(275,160), "reset":(275,200)}
static = {"Number of slices(max 16)": (120,30)}
numpos = (160,100)
warningpos = (160,130)
def updatenum(newnum,number,numhistory):
	global numhist
	global num
	lastchar = number[-1]
	number= lastchar +str(newnum)
#	numhist = [num]
	numhistory.pop(0)
	numhistory.append(number)
	num = number
	numhist = numhistory
	print newnum,number, numhistory
while running:
	time.sleep(0.001)
	screen.fill(black)
	#text for number of slices
	for text,position in static.items():
		display_text = small_font.render(text,True,white)
		rect = display_text.get_rect(center = position)
		screen.blit(display_text,rect)
	# text for options
	for text,position in buttons.items():
		display_sel = small_font.render(text,True,white)
		selrect = display_sel.get_rect(center = position)
		screen.blit(display_sel,selrect)
	# getting the number
	for event in pygame.event.get():
		if event.type == pygame.QUIT:
			sys.exit()
		elif event.type == pygame.MOUSEBUTTONUP:
			pos = pygame.mouse.get_pos()
			xpos, ypos = pos
			if ypos>140 and ypos<180: #torow
				if xpos > 5 and xpos <50:
					updatenum(1,num,numhist)
				elif xpos > 50 and xpos < 100:
					updatenum(2,num,numhist)
				elif xpos >100 and xpos <150:
					updatenum(3,num,numhist)
				elif xpos > 150 and xpos <200:
					updatenum(4,num,numhist)
				elif xpos > 200 and xpos <250:
					updatenum(5,num,numhist)
				elif xpos > 250 and xpos <300:
					# something to confirm and move on to camera
					print('confirm')
					running = 0
			elif ypos > 180 and ypos <220:
				if xpos > 5 and xpos <50:
                                        updatenum(6,num,numhist)
                                elif xpos > 50 and xpos < 100:
                                        updatenum(7,num,numhist)
                                elif xpos >100 and xpos <150:
                                        updatenum(8,num,numhist)
                                elif xpos > 150 and xpos <200:
                                        updatenum(9,num,numhist)
                                elif xpos > 200 and xpos <250:
                                        updatenum(0,num,numhist)
                                elif xpos > 250 and xpos <300:
                                        num = "00" # reset number
					numhist = [num]
					print('reset')
# update number
        display_num = big_font.render(numhist[0],True,white)
        num_rect = display_num.get_rect(center = numpos)
        screen.blit(display_num,num_rect)


	if int(num)>16:
		num = '01'
		numhist = [num]
		warningtext = 'input number <=16'
	elif int(num)==0:
		num = '01'
		numhist = [num]
		warningtext = 'input number > 1'
	else:
		warningtext = 'max slices:16'
	displaywarning = small_font.render(warningtext,True,white)
	warningrect = displaywarning.get_rect(center = warningpos)
	screen.blit(displaywarning,warningrect)
	pygame.display.flip()
print('clean up')
GPIO.cleanup()

from cakegui import num

print(num)
num = int(num)
# calculate based on calibration
# how many rotation does it make based on the

# angle for each slice is 360 deg/n
# 12 rotation of small gear = 1 overall rotation of the whole thing
# angle for each slice
numrot = 12/num
overallangle = 360/num
smallgearangle = overallangle*12 #small gear should rotate perslice

# INITIALIZE DC MOTORS
import RPi.GPIO as GPIO
import time
import os
import sys, pygame
# time.sleep(10)
# showing on piTFT
os.putenv('SDL_VIDEORIVER','fbcon')
os.putenv('SDL_FBDEV','/dev/fb0')
os.putenv('SDL_MOUSEDRV','TSLIB')
os.putenv('SDL_MOUSEDEV','/dev/input/touchscreen')

# set up screen display
pygame.init()
size = width, height = 320, 240
screen = pygame.display.set_mode(size)
size = width, height = 320, 240
screen = pygame.display.set_mode(size)
white = 255, 255, 255
black = 0, 0, 0
red = 255, 0, 0
green = 0, 255, 0
big_font = pygame.font.Font(None,40)
game_font = pygame.font.Font(None,25)
pygame.mouse.set_visible(False)

words = {"cake is cutting!": (160, 50),
	"quit": (280, 220)
}
redpanic = True
ptext = "STOP"
color = red
tlim = 60
GPIO.setmode(GPIO.BCM)
# force quit
GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP)
def GPIO17_callback(channel):
	global running
	print("quit with bail out button")
	running = 0
	GPIO.cleanup()
# set up GPIO pins for buttons
GPIO.add_event_detect(17,GPIO.FALLING,callback=GPIO17_callback,bouncetime=400)
# set up motor pins
GPIO.setup(5, GPIO.OUT) #AI2
GPIO.setup(6, GPIO.OUT) #AI1
GPIO.setup(26, GPIO.OUT) #PWMA
GPIO.setup(13, GPIO.OUT) #pservo
#GPIO.setup(20, GPIO.OUT)
#GPIO.setup(21, GPIO.OUT)
# set output to low for both initially
GPIO.output(5,0)
GPIO.output(6,0)
pdc = GPIO.PWM(26, 100)
pdc.start(20)
#servo
servofreq = 1/(0.020 + 0.0016)
pservo = GPIO.PWM(13,servofreq)
#conversmallgearangle into dc cycle using continuous servo
redpanic = True
changed = False
dc = False
servo = False
started = False
trot = 0.88
rottime = trot*numrot
cuttime = 4
pausetime = 0.25
phase = [1,0,0,0,0,0]
pausestart = 0
cycle = 1*rottime + 2*cuttime + 3*pausetime
start = time.time()
offsettime = start
slice = 0
j = 0
print("starting to cut " + str(num) + " slices")
while time.time() < start+tlim and slice<num:
        time.sleep(0.001)
        screen.fill(black)
        display_p = big_font.render(ptext,True,white)
        p_rect = display_p.get_rect(center=(160,120))
        pygame.draw.circle(screen,color,(160,120),40)
        screen.blit(display_p,p_rect)
        for text, position in words.items():
                display_text = game_font.render(text, True, white)
                rect = display_text.get_rect(center=position)
                screen.blit(display_text, rect)
        for event in pygame.event.get():
                if event.type == pygame.MOUSEBUTTONUP:
                        pos = pygame.mouse.get_pos()
                        #print(pos)
                        xpos, ypos = pos
                        if p_rect.collidepoint(pos):
                                print("panic toggle")
                                redpanic = not redpanic
                        elif (xpos>260) and (ypos>200):
                                print("quitting")
                                tlim = 0
        pygame.display.flip()

        #panic button
        if redpanic:
                color = red
                ptext = "STOP"
                if changed:
			if dc:
                        	pdc.ChangeDutyCycle(20)
			elif servo:
				pservo.ChangeFrequency(1/0.0216)
				pservo.start(1.6/21.6*100)
                        changed = False
			offsettime = offsettime + time.time()-pausestart
        else:
                color = green
                ptext = "RESUME"
                if not changed:
                        pdc.stop()
			pservo.stop()
                        changed = True
			pausestart = time.time()

	if ((time.time()-offsettime) % cycle)<rottime: #servo
		servo = True
		if redpanic and phase[0] == 1:
			pservo.ChangeFrequency(1/0.0216)
			pservo.start(1.6/21.6*100)
			phase[0] = 0
			phase[1] = 1
	elif ((time.time()-offsettime) % cycle)<(rottime+pausetime): #pause
		pservo.stop()
		servo = False
       	        if redpanic and phase[1] == 1:
       	                phase[1] = 0
       	                phase[2] = 1
       	elif ((time.time()-offsettime) % cycle)<(rottime+pausetime + cuttime): #backward
		GPIO.output(5,0)
		GPIO.output(6,1)
		dc = True
		if changed:
			print(dc)
			pdc.ChangeDutyCycle(20)
       	        if redpanic and phase[2] == 1:
			pdc.start(30)
       	                phase[2] = 0
       	                phase[3] = 1
       	elif ((time.time()-offsettime) % cycle)<(rottime+2*pausetime+cuttime): #pivot left
       	        GPIO.output(5,0)
		GPIO.output(6,0)
		dc = False
       	        if redpanic and phase[3] == 1:
       	                phase[3] = 0
       	                phase[4] = 1
       	elif ((time.time()-offsettime) % cycle)<(rottime+2*pausetime+2*cuttime): #stop
		GPIO.output(6,0)
		GPIO.output(5,1)
		dc = True
       	        if redpanic and phase[4] == 1:
			pdc.start(20)
       	                phase[4] = 0
       	                phase[5] = 1
	else:
		dc = False
		GPIO.output(5,0)
		GPIO.output(6,0)
       	        if redpanic and phase[5] == 1:
                       	phase[5] = 0
                       	phase[0] = 1
			slice = slice+1
	pygame.display.flip()
print("Done!")
GPIO.cleanup()